import os
import halcon as ha
from  util_3d  import *
class V3D():

    def __init__(self):
        self.acq_handle = ha.open_framegrabber('USB3Vision', 0, 0, 0, 0, 0, 0,
                                               'progressive', -1, 'default', -1, 'false', 'default',
                                               '2BDF00964452_Hikrobot_MVCA01321UC', 0, -1,)

        ha.grab_image_start(self.acq_handle, -1)
        self.Image = ha.grab_image_async(self.acq_handle, -1)
        self.window_image = self.open_window(1280, 1024)
        ha.disp_obj(self.Image, self.window_image)

    def update_image(self, disp_flag):
        self.Image = ha.grab_image(self.acq_handle)
        if disp_flag == True:
            ha.disp_obj(self.Image, self.window_image)
        return self.Image

    def get_image(self):
        return self.Image

    def update_image_loop(self):
        while True:
            self.Image = ha.grab_image(self.acq_handle)
            ha.disp_obj(self.Image, self.window_image)


    def gen_cam_t_calib(self):
        self.Image = ha.grab_image(self.acq_handle)
        ha.disp_obj(self.Image, self.window_image)
        CamParam = ha.read_cam_par('C:/Users/k/Desktop/hybrid_robot/vision/hand_eye2/inner_16mm.cal')
        CaltabName = 'caltab_30mm.descr'
        CalibDataID = ha.create_calib_data('calibration_object', 1, 1)
        ha.set_calib_data_cam_param(CalibDataID, 0, [], CamParam)
        ha.set_calib_data_calib_object(CalibDataID, 0, CaltabName)
        PoseForCalibrationPlate = xyzrpy_to_T(0,0,0,0,0,0)
        for i in range(10):
            try:
                ha.find_calib_object(self.Image, CalibDataID, 0, 0, 1, [], [])
                Caltab = ha.get_calib_data_observ_contours(CalibDataID, 'caltab', 0, 0, 1)
                RCoord, CCoord, Index, PoseForCalibrationPlate = ha.get_calib_data_observ_points(CalibDataID, 0, 0, 1)
                # ha.set_color(self.window_image, 'green')
                # ha.disp_caltab(Caltab, self.window_image)
                return True, halcon_pose_to_T(PoseForCalibrationPlate)
            except:
                print('error')
                if i == 9:
                    return False, PoseForCalibrationPlate

    def open_window(self, width, height, row=0, col=0, win_id=0):
        if os.name == 'nt':
            ha.set_system('use_window_thread', 'true')
        window = ha.open_window(
            row=row,
            column=col,
            width=width,
            height=height,
            father_window=win_id,
            mode='visible',
            machine=''
        )
        ha.set_draw(window, 'fill')
        ha.set_line_width(window, 2)
        ha.set_color(window, 'green')
        return window



